#include "axisgroup.h"
#include <string.h>
#include <cxxabi.h>
#include "errcode.h"
#include "zuc/mot_cfg.h"
#include "axis/axis.h"
#include "kine/kine.h"
#include "dyna/jaka_dyn_intf.h"
#include "log/zuclog.h"
#include "fsm_booting.h"
#include "fsm_disable.h"
#include "fsm_standby.h"
#include "fsm_errorstop.h"
#include "errcode.h"
#include "rtdev/rtrobot.h"
#include "axisgroup_set.h"

#include <shared_mutex>

#define AXISGROUP_LOG_INFO(fmt, ...) zuclog_info("AxisGroup %d", fmt, group_id_, ##__VA_ARGS__)
#define AXISGROUP_LOG_ERR(fmt, ...) zuclog_error("AxisGroup %d", fmt, group_id_, ##__VA_ARGS__)

const size_t SERVO_HOME_COUNT = 3;

using axisgroup::AxisGroup;
using namespace std;

const char* AxisGroup::run_state_string(AxisGroup::FsmState state)
{
    switch (state)
    {
    case AxisGroup::FsmState::Booting:
        return "Booting";
    case AxisGroup::FsmState::Disabled:
        return "Disabled";
    case AxisGroup::FsmState::Standby:
        return "Standby";
    case AxisGroup::FsmState::Moving:
        return "Moving";
    case AxisGroup::FsmState::Stopping:
        return "Stopping";
    case AxisGroup::FsmState::ErrorStop:
        return "ErrorStop";
    default:
        return "Unknown";
    }
}

AxisGroup::AxisGroup(uint32_t serial_num) : serial_num_(serial_num), sim_serial_num_(serial_num)
{
    init_fsm();
    reset();
    memset(&cmd_, 0, sizeof(cmd_));
    memset(&config_, 0, sizeof(config_));
    cmd_.home_joint = -1;

    group_id_ = 0;
    sim_flg_ = false;
    config_.action_after_boot_timeout = SHUTDOWN_AFTER_BOOT_TIMEOUT;
    config_.cyclictime_sec = 0.008;
    config_.rapid_scale = 1.0;
    config_.net_feed_scale = 1.0;
    config_.safety.max_tcp_vel = 2000;
    for (size_t i = 0; i < ::RobJointVal::MAX_AXIS; i++)
    {
        config_.safety.clsn_sensitivity_lv[i] = 0;
        config_.safety.jpos_eps[i] = 80;
        config_.safety.max_jpos[i] = 360;
        config_.safety.min_jpos[i] = -360;
    }
    last_fsm_sta_ = fsm_sta_ = FsmState::Booting;
}

int AxisGroup::reset()
{
    joints.clear();
    reset_kine_dyn();
    return 0;
}

// 程序启动时的boot_init, 读取了配置文件中的机型号
int axisgroup::AxisGroup::boot_init(int id, std::shared_ptr<rtdev::RtRobot> rtrobot)
{
    rtrobot_ = rtrobot;                                    // 绑定rtrobot
    set_intf_ = std::make_shared<RobotSetIntf>(id, this);  // 实例时，从配置文件与机型文件中加载配置
    return boot_init(set_intf_->serial_num_, false);
}

int axisgroup::AxisGroup::boot_init(int serial_num, bool is_rt)
{
    AXISGROUP_LOG_INFO("boot_init serial is %d from %s", serial_num, is_rt ? "RT" : "CFG");
    serial_num_ = serial_num;  // 切换机型号
    set_intf_->load_config(serial_num_);

    reset_links();  // 重新根据机型文件绑定轴
    int axis_start_id = set_intf_->active_config_.robot_axis_cfg.axis_start_id;
    auto bus_name = set_intf_->active_config_.robot_axis_cfg.bus_chn;
    auto dev_id = set_intf_->active_config_.robot_axis_cfg.bus_devid;
    int rob_axis_cnt = set_intf_->active_config_.robot_axis_cfg.axis_num;
    for (int i = axis_start_id; i < axis_start_id + rob_axis_cnt; i++)
    {
        auto a = axis::AxisMgr::get().axis(i);
        if (!a)
        {
            break;
        }
        if (a->get_linked_grp_id() != -1 && a->get_linked_grp_id() != group_id_)
        {
            AXISGROUP_LOG_ERR("Axis %d is already bind to group %d", i, a->get_linked_grp_id());
            return -1;
        }
        //FIXME:检查devid的长度是否合法
        a->boot_init(bus_name, dev_id[i - axis_start_id]);  // 重新bind bus
        link_joint(a.get());
    }

    reset_kine_dyn();  // 构型可能发生变化，重新建立
    set_intf_->sync_robot_config();

    last_fsm_sta_ = fsm_sta_ = FsmState::Disabled;
    return 0;
}

void AxisGroup::init_fsm()
{
    last_fsm_sta_ = fsm_sta_ = FsmState::Disabled;
    fsm_sta_booting_.reset(new FsmBooting(this));
    fsm_sta_disabled_.reset(new FsmDisable(this));
    fsm_sta_standby_.reset(new FsmStandby(this));
    fsm_sta_errorstop_.reset(new FsmErrorStop(this));
}

std::shared_ptr<rtdev::RtRobot> axisgroup::AxisGroup::RtRobot() { return rtrobot_; }

// 注意这个reset会导致kine、dyn内部所有的参数丢失，需要重新设置
int AxisGroup::reset_kine_dyn()
{
    //运动学初始化
    kine_ = kine::create_kine(axisgroup::RobotSetIntf::get_module_type(serial_num_));

    //动力学初始化
    JAKADynIntf::JakaRobotType dyna_type = JAKADynIntf::JakaRobotType::MODEL_ZU;
    switch (axisgroup::RobotSetIntf::get_module_type(serial_num_))
    {
    case kine::KINETYPE_ZU:
        dyna_type = JAKADynIntf::JakaRobotType::MODEL_ZU;
        break;
    case kine::KINETYPE_MINI:
        dyna_type = JAKADynIntf::JakaRobotType::MODEL_MC;
        break;
    case kine::KINETYPE_ATOM:
        dyna_type = JAKADynIntf::JakaRobotType::MODEL_ATOM;
        break;
    case kine::KINETYPE_MAX:
        dyna_type = JAKADynIntf::JakaRobotType::MODEL_MAX;
        break;
    default:
        break;
    }
    dyna_ = std::make_shared<JAKADynIntf>(dyna_type);
    return 0;
}

// 机型更新时，需要重新reset

int axisgroup::AxisGroup::shutdown()
{
    cmd_.power = false;
    change_fsm_stat(axisgroup::AxisGroup::FsmState::Disabled);  // 下电
    return 0;
}
std::vector<uint32_t> AxisGroup::get_joint_ids()
{
    std::vector<uint32_t> jids;
    for (auto& j : joints) { jids.push_back(j->get_id()); }
    return jids;
}

void AxisGroup::reset_links()
{
    joints.clear();
    if (RtRobot().get())
    {
        RtRobot()->reset_servo();
    }
}

AxisGroup::FsmState axisgroup::AxisGroup::get_state() { return fsm_sta_; }
void axisgroup::AxisGroup::set_sim(bool sim, uint32_t serial_num)
{
    if (sim)
    {
        if (sim_serial_num_ != serial_num && serial_num != 0)
        {
            AXISGROUP_LOG_INFO("Sim robot change from %d to %d", sim_serial_num_, serial_num);
            sim_serial_num_ = serial_num;
            change_to_booting();
            boot_init(sim_serial_num_, 0);
        }
    }
    if (sim_flg_ != sim && serial_num != 0)
    {
        AXISGROUP_LOG_INFO("Run mode change from %s to %s, robot serial : %d", sim_flg_ ? "SIM" : "REAL", sim ? "SIM" : "REAL", serial_num);
        reset_kine_dyn();  // 机型可能发生变化，重新适配机型
    }
    sim_flg_ = sim;
    for (auto& j : joints) { j->set_sim(sim); }
}

bool axisgroup::AxisGroup::is_enabled() const
{
    switch (fsm_sta_)
    {
    case FsmState::Standby:
    case FsmState::Moving:
    case FsmState::Stopping:
        return true;
    case FsmState::ErrorStop:
        return cmd_.enable;  // #TODO
    default:
        return false;
    }
}

int AxisGroup::link_joint(axis::Axis* joint)
{
    if (fsm_sta_ != FsmState::Booting)
    {
        return -3;
    }
    if (joints.size() > MAX_AXIS)
    {
        return -1;
    }
    for (auto& j : joints)
    {
        if (j->get_id() == joint->get_id())
        {
            return -2;
        }
    }
    joint->link_grp(group_id_);
    joints.push_back(joint);
    std::sort(joints.begin(), joints.end(), [](axis::Axis* a, axis::Axis* b) { return a->get_id() < b->get_id(); });

    if (RtRobot())
    {
        RtRobot()->link_servo(joint->servo_, joint->get_id());  // #FIXME 非线程安全
    }
    return 0;
}

void AxisGroup::change_fsm_stat(FsmState new_sta)
{
    if (new_sta != fsm_sta_)
    {
        AXISGROUP_LOG_INFO("CMD:Runstat change from %s to %s", run_state_string(fsm_sta_), run_state_string(new_sta));
    }
    fsm_sta_ = new_sta;
}
void AxisGroup::change_to_errstat(RobEvCode error_code, bool reset_enable)
{
    if (reset_enable)
    {
        cmd_.enable = false;
    }
    change_fsm_stat(FsmState::ErrorStop);
    AXISGROUP_LOG_INFO("CMD: change to ErrorStop, ecode=%2x reseten=%d", (uint16_t)error_code, reset_enable);
    uint32_t ec = ROB_EV(error_code, group_id_);
    //TODO:压入错误码
    if (ec)
    {
        // state_.error_code_ = ec;
    }
}

void AxisGroup::update_fsm()
{
    for (size_t i = 0; i < joints.size(); i++) { joints[i]->task_update(0.008); }  //#FIXME

    bool is_new_sta = false;
    if (last_fsm_sta_ != fsm_sta_)
    {
        last_fsm_sta_ = fsm_sta_;
        is_new_sta = true;
    }

    //更新状态
    switch (fsm_sta_)
    {
    case FsmState::Booting:
        if (is_new_sta)
        {
            fsm_sta_booting_->reset();
        }
        fsm_sta_booting_->update(is_new_sta);
        break;
    case FsmState::Disabled:
        if (is_new_sta)
        {
            fsm_sta_disabled_->reset();
        }
        fsm_sta_disabled_->update(is_new_sta);
        break;
    case FsmState::Standby:
        if (is_new_sta)
        {
            fsm_sta_standby_->reset();
        }
        fsm_sta_standby_->update(is_new_sta);
        break;
    case FsmState::Moving:
        fsm_state_moving(is_new_sta);
        break;
    case FsmState::Stopping:
        fsm_state_stopping(is_new_sta);
        break;
    case FsmState::ErrorStop:
        if (is_new_sta)
        {
            fsm_sta_errorstop_->reset();
        }
        fsm_sta_errorstop_->update(is_new_sta);
        break;
    default:
        break;
    }

    if (last_fsm_sta_ != fsm_sta_)
    {
        AXISGROUP_LOG_INFO("Runstat change from %s to %s", run_state_string(last_fsm_sta_), run_state_string(fsm_sta_));
    }
}

void axisgroup::AxisGroup::change_to_booting() { change_fsm_stat(FsmState::Booting); }

void axisgroup::AxisGroup::reset_joint_planner()
{
    // booting 的时候阻塞太久，导致planner一直在预测，故booting后清除预测状态
    AXISGROUP_LOG_INFO("Reset Planner");
    for (size_t i = 0; i < joint_num(); i++) { joints[i]->reset_planner(); }
}

int AxisGroup::fsm_state_standby(bool first)
{
    if (!cmd_.enable)
    {
        //任意轴是Disable状态就切换Disable
        bool any_disabled = false;
        for (auto& joint : joints)
        {
            if (joint->run_state() == axis::Axis::RunState::Disable)
            {
                any_disabled = true;
                break;
            }
        }
        if (any_disabled)
        {
            change_fsm_stat(FsmState::Disabled);
        }
    }
    return 0;
}
int AxisGroup::fsm_state_moving(bool first) { return -1; }
int AxisGroup::fsm_state_stopping(bool first) { return -1; }
int AxisGroup::fsm_state_error_stop(bool first) { return -1; }

bool axisgroup::AxisGroup::is_homing() const { return cmd_.home_joint > -1; }

bool axisgroup::AxisGroup::is_homed() const
{
    if (cmd_.home_joint < 0)
    {
        return true;
    }
    else
    {
        return joints[cmd_.home_joint]->is_homed();
    }
}

void axisgroup::AxisGroup::set_cycle_time(double mot_sec, double servo_sec)
{
    AXISGROUP_LOG_INFO("Robot Motion Cycle: %lf, Servo Cycle: %lf", mot_sec, servo_sec);
    config_.cyclictime_sec = mot_sec;
    config_.servo_time_sec = servo_sec;
    for (size_t i = 0; i < joint_num(); i++) { joints[i]->set_traj_time(servo_sec, mot_sec); }
}

void AxisGroup::do_enable(bool en)
{
    if (cmd_.enable != en)
    {
        AXISGROUP_LOG_INFO("do_enable %s", en ? "enable" : "disable");
        cmd_.enable = en;
    }
    for (size_t i = 0; i < joints.size(); i++) { joints[i]->enable(en); }
}

void axisgroup::AxisGroup::do_power(bool en)
{
    if (cmd_.power != en)
    {
        AXISGROUP_LOG_INFO("do_power %s", en ? "power on" : "power off");
        cmd_.power = en;
    }
    if (en)
    {
        change_to_booting();
    }
    else if (!en)
    {
        change_fsm_stat(FsmState::Disabled);
    }
}

void axisgroup::AxisGroup::servo_home_end_action()
{
    AXISGROUP_LOG_INFO("Joint %d servo home end!", cmd_.home_joint);
    // servo home 结束后，需要恢复到位置模式
    joints[cmd_.home_joint]->set_axis_tracking_mode(axis::Axis::OpMode::OP_MODE_CSP);
    joints[cmd_.home_joint]->set_torque_forward(true);  // 关闭前馈
    cmd_.home_joint = -1;
}

void axisgroup::AxisGroup::do_servo_home(int joint)
{
    cmd_.home_joint = joint;
    cmd_.home_wait_count = SERVO_HOME_COUNT;
    // servo home 结束后，需要恢复到位置模式
    joints[cmd_.home_joint]->set_home();
    joints[joint]->set_axis_tracking_mode(axis::Axis::OpMode::OP_MODE_HOME);
    joints[joint]->set_torque_forward(false);  // 关闭前馈
    AXISGROUP_LOG_INFO("Joint %d servo home start!", cmd_.home_joint);
}

axisgroup::AgConfig::RobotConfig axisgroup::AxisGroup::get_robot_active_config() const { return set_intf_->active_config_; }